import math
import serial#type: ignore
import time
from config import portWheel
class wheelMotion:
    def __init__(self,serial):
        self.serial=serial
    def move(self,x,y,ahead=0.2):
        r = math.sqrt(x**2 + y**2)
        theta = math.atan2(y, x)
        new_r = max(r - ahead, 0)
        new_x = new_r * math.cos(theta)
        new_y = new_r * math.sin(theta)
        print(f"moving to x={x} y={y},ahead={ahead},new_x={new_x},new_y={new_y}")
        self.serial.write(f"move {new_x} {new_y}".encode())
    def moveForward(self,message=0):
        message=str(message)
        self.serial.write(message.encode('utf-8'))
        print(f"已发送消息：{message}")
        response = self.serial.readline().decode('utf-8').strip()
        print(f"收到响应：{response}")
if __name__ == "__main__":
    port = portWheel  
    baud_rate = 9600 
    timeout = 1  
    ser = serial.Serial(port, baud_rate, timeout=timeout)
    wheelmotion = wheelMotion(ser)
    wheelmotion.moveForward(1)
